//#include <asm/types.h>
#include <linux/videodev2.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <iostream>
#include <cstring>

#include "CameraInterface.h"
#include "LFOSCommon.h"

#if SIM
#include <sstream>
#include <cstdlib>
#include <fstream>
#endif

using namespace std;

CameraInterface::CameraInterface()
{
	timeBetweenFrames = 0;
	timeOfLastCapture = 0;
	fdCamera = -1;
}


void CameraInterface::SetTimeBetweenFrames(unsigned int milliseconds)
{
	timeBetweenFrames = milliseconds;
}

unsigned int CameraInterface::GetTimeBetweenFrames()
{
	return timeBetweenFrames;
}


unsigned char* CameraInterface::CaptureFrame()
{
	unsigned char buffer[2*PIXELS];
    unsigned long timeMilliSeconds;
	struct timeval currentTime;

	gettimeofday(&currentTime, 0);


    timeMilliSeconds = currentTime.tv_sec*1000 + currentTime.tv_usec/1000;

    if((timeMilliSeconds - timeOfLastCapture) >= timeBetweenFrames)
	{
#if SIM
    	static int count = 0;
    	stringstream fileName;
    	int nColors;
    	ifstream inFile;

    	fileName << "frame_" << count << ".bmp";

    	inFile.open(fileName.str().c_str(), ios::in | ios::binary);
    	if(inFile.fail())
    	{
    		cout << fileName.str() << " does not exist\n";
    		exit(1);
    	}

    	inFile.seekg(46);
    	inFile.read((char*)buffer, 4);
    	nColors = buffer[0] | (buffer[1] << 8) |
    			(buffer[2] << 16) | (buffer[3] << 24);

    	inFile.seekg(54+4*nColors);

    	inFile.read((char*)buffer, PIXELS);


    	//image stored upside down - need to invert rows
    	for(int y=0; y < ROWS; y++)
    		for(int x=0; x < COLS; x++)
    		{
    			frameData[ROW_COL_TO_INDEX(x,ROWS-y-1)] = buffer[ROW_COL_TO_INDEX(x,y)];
    		}
    	++count;
#else

		read(fdCamera, buffer, 2*PIXELS);

		//Rotate image 180 degrees and store gray scale (first PIXEL bytes of array)
		for(int i = 0; i < PIXELS; ++i)
		{
			frameData[PIXELS-1-i] = buffer[i];
		}
#endif

        timeOfLastCapture = timeMilliSeconds;

		return frameData;
	}
	else
		return 0;
}

bool CameraInterface::Initialize(unsigned int time)
{
    //struct v4l2_capability cap;
	struct v4l2_format fmt;
	char fourcc[5]={0,0,0,0,0};
	unsigned char buffer[2*PIXELS];

#if SIM
	fdCamera = 0;
#else
    memset(&fmt, 0, sizeof(fmt));

    fmt.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.width = COLS;
    fmt.fmt.pix.height= ROWS;

    fdCamera = open("/dev/video0", O_RDWR);

	//ioctl(fd, VIDIOC_QUERYCAP, &cap);

	ioctl(fdCamera, VIDIOC_S_FMT, &fmt);

	//memmove(fourcc,&fmt.fmt.pix.pixelformat,4);

	//Read one frame since first frame is always garbage
		read(fdCamera, buffer, 2*PIXELS);
#endif


	timeBetweenFrames = time;

    return fdCamera != -1;
}

